﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using NIP.ControllerInterface;
using NIP.DynamicControl.Constants;
using NIP.DynamicControl.Environ;


namespace NIP.DynamicControl.InverseKinematics
{
    public class Inv_K
    {
        public Inv_K()
        {
            Console.WriteLine("Inv_K engine created");
        }

        ~Inv_K()
        {
            Console.WriteLine("Inv_K engine destroyed");
        }
        
        

        public static bool solveMoveToJointAngles(PathPoint tempGoal)
        {
            
            if (XYZisfeasible(tempGoal))
            {
                double X = tempGoal.pos[0];
                double Y = tempGoal.pos[1];
                double Z = tempGoal.pos[2];
                double thetap = gridconst.toRadians(tempGoal.getPitch());
                double theta5 = tempGoal.getRoll();
                double d5 = gridconst.D5grip;
                double d6 = gridconst.D6grip;
                double offsetXY = gridconst.L4 - (d6 * Math.Sin(theta5));
                double theta1 = 0.0;
                double theta2 = 0.0;
                double theta3 = 0.0;
                double theta4 = 0.0;
                double L0 = gridconst.L0;
                double L1 = gridconst.L1;
                double L2 = gridconst.L2;
                double L3 = gridconst.L3;
                double L23max = gridconst.L23max;
                double L23min = Math.Sqrt((L2 * L2) + (L3 * L3) - (2.0 * L2 * L3 * Math.Cos(gridconst.toRadians(Math.Abs(gridconst.theta3min)))));
                double XY = Math.Sqrt((X * X) + (Y * Y));
                double ShoulderToPointZ = Z - L1;
                double ShoulderToPointXY = Math.Sqrt( (XY * XY)  - (offsetXY * offsetXY)) - L0 ;
                double ShoulderToPointXYZ = Math.Sqrt(((ShoulderToPointXY) * (ShoulderToPointXY)) + (ShoulderToPointZ * ShoulderToPointZ));
                double Cp = Math.Cos(thetap);
                double Sp = Math.Sin(thetap);
                double ShoulderToWristXY = ShoulderToPointXY - (d5 * Cp) - (d6 * Sp);
                double ShoulderToWristZ = Z - (d5 * Sp) + (d6 * Cp) - L1;
                double ShoulderToWristXYZ = Math.Sqrt((ShoulderToWristXY * ShoulderToWristXY) + (ShoulderToWristZ * ShoulderToWristZ));
                if (ShoulderToPointXYZ > (L23max + d5))
                {
                    Debug.WriteLine("Cannot reach position " + X + "," + Y + "," + Z);
                    return false;
                }
                double EEap;
                double EEop;
                double EEhp;
                double EEaw;
                double EEow;
                double EEhw;
                theta1 = Math.Atan2(Y, X) + Math.Asin(offsetXY / XY);
                theta1 = gridconst.toDegrees(theta1);
                if (ShoulderToWristXYZ > L23max) // too far for pitch
                {
                    //Debug.WriteLine("XYZ " + X + "," + Y + "," + Z + " too far for current grip pitch");
                    //
                    EEop = ShoulderToPointZ;
                    EEap = ShoulderToPointXY;
                    EEhp = ShoulderToPointXYZ;
                    double theta2a = Math.Atan2(EEop, EEap);
                    double thetapa = Math.Acos(((EEhp * EEhp) + (d5 * d5) - ((L23max - 40.0) * (L23max - 40.0))) / (2.0 * EEhp * d5));
                    thetap = gridconst.toDegrees(theta2a - thetapa);
                    theta2a = gridconst.toDegrees(theta2a);
                    thetapa = gridconst.toDegrees(thetapa);
                    Debug.WriteLine("Can reach by changing wrist pitch to " + (thetap) + " degrees");
                    theta3 = Math.PI;// Math.Acos(((L2 * L2) + (L3 * L3) - (L23max * L23max)) / (2.0 * L2 * L3));
                    theta3 = gridconst.toDegrees(theta3);
                    //Debug.Write("Which gives a theta3 of " + (theta3 - 180.0) + " degrees");
                    if ((180.0 - theta3) < gridconst.theta3min)
                    {
                        
                        //throw new Exception("theta 3 limit");
                    }
                    Cp = Math.Cos(gridconst.toRadians(thetap));
                    Sp = Math.Sin(gridconst.toRadians(thetap));
                    EEow = Z - (d5 * Sp) + (d6 * Cp) - L1;
                    EEaw = ShoulderToPointXY - (d5 * Cp) - (d6 * Sp);
                    EEhw = Math.Sqrt((EEow * EEow) + (EEaw * EEaw));
                    double theta2c = Math.Atan2(EEow, EEaw);
                    double theta2b = 0.0;// Math.Acos(((L2 * L2) + (EEhw * EEhw) - (d5 * d5)) / (2.0 * L2 * EEhw)); don't need for this case = 0 degrees
                    theta2b = gridconst.toDegrees(theta2b);
                    theta2c = gridconst.toDegrees(theta2c);
                    //Debug.Write(", a theta2 of " + (theta2b + theta2c) + " degrees");
                    if (((-theta2b - theta2c) < gridconst.theta2min) || ((-theta2b - theta2c) > gridconst.theta2max))
                    {

                        //throw new Exception("theta 2 limit");
                    }
                    theta2 = -(theta2b + theta2c);
                    theta4 = 360.0 + theta2 - theta3 + thetap;
                    theta3 = (180.0 - theta3);
                    theta4 = (180.0 - theta4);
                    //Debug.WriteLine(" and theta4 of " + (theta4) + " degrees");
                    if ((theta4 < gridconst.theta4min) || (theta4 > gridconst.theta4max))
                    {

                        //throw new Exception("theta 4 limit");
                    }



                }
                else if (ShoulderToWristXYZ < L23min) // too close for pitch
                {
                    //Debug.WriteLine("XYZ " + X + "," + Y + "," + Z + " too close for current grip pitch");
                    //
                    EEop = ShoulderToPointZ;
                    EEap = ShoulderToPointXY;
                    EEhp = ShoulderToPointXYZ;
                    double theta2a = Math.Atan2(EEop, EEap);
                    double thetapa = Math.Acos(((EEhp * EEhp) + (d5 * d5) - (L23min * L23min)) / (2.0 * EEhp * d5));
                    thetap = gridconst.toDegrees(theta2a - thetapa);
                    theta2a = gridconst.toDegrees(theta2a);
                    thetapa = gridconst.toDegrees(thetapa);
                    Debug.WriteLine("Can reach by changing wrist pitch to " + (thetap) + " degrees");
                    theta3 = Math.Acos(((L2 * L2) + (L3 * L3) - (L23min * L23min)) / (2.0 * L2 * L3));
                    theta3 = gridconst.toDegrees(theta3);
                    //Debug.Write("Which gives a theta3 of " + (theta3 - 180.0) + " degrees");
                    if ((180.0 - theta3) < gridconst.theta3min)
                    {
                        
                        //throw new Exception("theta 3 limit");
                    }
                    Cp = Math.Cos(gridconst.toRadians(thetap));
                    Sp = Math.Sin(gridconst.toRadians(thetap));
                    EEow = Z - (d5 * Sp) + (d6 * Cp) - L1;
                    EEaw = ShoulderToPointXY - (d5 * Cp) - (d6 * Sp);
                    EEhw = Math.Sqrt((EEow * EEow) + (EEaw * EEaw));
                    double theta2c = Math.Atan2(EEow, EEaw);
                    double theta2b = Math.Acos(((L2 * L2) + (EEhw * EEhw) - (L3 * L3)) / (2.0 * L2 * EEhw));
                    theta2b = gridconst.toDegrees(theta2b);
                    theta2c = gridconst.toDegrees(theta2c);
                    //Debug.Write(", a theta2 of " + (theta2b + theta2c) + " degrees");
                    if (((-theta2b - theta2c) < gridconst.theta2min) || ((-theta2b - theta2c) > gridconst.theta2max))
                    {

                        //throw new Exception("theta 2 limit");
                    }
                    theta2 = -(theta2b + theta2c);
                    theta4 = 360.0 + theta2 - theta3 + thetap;
                    theta3 = (180.0 - theta3);
                    theta4 = (180.0 - theta4);
                    //Debug.WriteLine(" and theta4 of " + (theta4) + " degrees");
                    if ((theta4 < gridconst.theta4min) || (theta4 > gridconst.theta4max))
                    {

                        //throw new Exception("theta 4 limit");
                    }
                }

                else // can reach with pitch
                {
                    EEop = ShoulderToPointZ;
                    EEap = ShoulderToPointXY;
                    EEhp = ShoulderToPointXYZ;
                    double theta2a = Math.Atan2(EEop, EEap);
                    thetap = gridconst.toDegrees(thetap);
                    Debug.WriteLine("Can reach with wrist pitch of " + (thetap) + " degrees");
                    theta3 = Math.Acos(((L2 * L2) + (L3 * L3) - (ShoulderToWristXYZ * ShoulderToWristXYZ)) / (2.0 * L2 * L3));
                    theta3 = gridconst.toDegrees(theta3);
                    //Debug.Write("Which gives a theta3 of " + (theta3 - 180.0) + " degrees");
                    if ((180.0 - theta3) < gridconst.theta3min)
                    {
                        //throw new Exception("theta 3 limit");
                    }
                    Cp = Math.Cos(gridconst.toRadians(thetap));
                    Sp = Math.Sin(gridconst.toRadians(thetap));
                    EEow = Z - (d5 * Sp) + (d6 * Cp) - L1;
                    EEaw = ShoulderToPointXY - (d5 * Cp) - (d6 * Sp);
                    EEhw = Math.Sqrt((EEow * EEow) + (EEaw * EEaw));
                    double theta2c = Math.Atan2(EEow, EEaw);
                    double theta2b = Math.Acos(((L2 * L2) + (EEhw * EEhw) - (L3 * L3)) / (2.0 * L2 * EEhw));
                    theta2b = gridconst.toDegrees(theta2b);
                    theta2c = gridconst.toDegrees(theta2c);
                    //Debug.Write(", a theta2 of " + (-theta2b - theta2c) + " degrees");
                    if (((-theta2b - theta2c) < gridconst.theta2min) || ((-theta2b - theta2c) > gridconst.theta2max))
                    {

                        //throw new Exception("theta 2 limit");
                    }
                    theta2 = -(theta2b + theta2c);
                    theta4 = 360.0 + theta2 - theta3 + thetap;
                    theta3 = (180.0 - theta3);
                    theta4 = (180.0 - theta4);
                    //Debug.WriteLine(" and theta4 of " + (theta4) + " degrees");
                    if ((theta4 < gridconst.theta4min) || (theta4 > gridconst.theta4max) )
                    {

                        //throw new Exception("theta 4 limit");
                    }
                    
                }

                // remove this section once inverse kinematics fixed
                if (theta3 < gridconst.theta3min)
                {
                    theta3 = gridconst.theta3min;
                }
                if (theta3 > gridconst.theta3max)
                {
                    theta3 = gridconst.theta3max;
                }
                if (theta4 < gridconst.theta4min) 
                {
                    theta4 = gridconst.theta4min;
                }
                if (theta4 > gridconst.theta4max)
                {
                    theta4 = gridconst.theta4max;
                }
                if (theta2 < gridconst.theta2min) 
                {
                    theta2 = gridconst.theta2min;
                }
                if(theta2 > gridconst.theta2max)
                {
                    theta2 = gridconst.theta2max;

                }

                                
                
                tempGoal.setJoint(0, theta1);
                tempGoal.setJoint(1, theta2);
                tempGoal.setJoint(2, theta3); //180.0 + for elbow down config
                tempGoal.setJoint(3, theta4);
                tempGoal.setRoll(theta5);
                Debug.Write("X:" + tempGoal.pos[0] + " Y:" + tempGoal.pos[1] + " Z:" + tempGoal.pos[2]);
                Debug.Write(" J0:" + tempGoal.getJoint(0) + " J1:" + tempGoal.getJoint(1) + " J2:" + tempGoal.getJoint(2));
                Debug.WriteLine(" J3:" + tempGoal.getJoint(3) + " J4:" + tempGoal.getJoint(4) + " Pitch:" + tempGoal.getPitch());

                return true;
            }
            else
            {
                Debug.WriteLine(tempGoal.pos[0] + "," + tempGoal.pos[1] + "," + tempGoal.pos[2] + " outside workspace");
                Console.WriteLine(tempGoal.pos[0] + "," + tempGoal.pos[1] + "," + tempGoal.pos[2] + " outside workspace");
                return false;
            }

        }
        public static bool solveLookAtJointAngles(PathPoint tempGoal)
        {
            double X = tempGoal.pos[0];
            double Y = tempGoal.pos[1];
            double Z = tempGoal.pos[2];
            double thetap = gridconst.toRadians(tempGoal.getPitch());
            double theta5 = gridconst.toRadians(tempGoal.getRoll()); 
            double dOffset = ((tempGoal.getOffset() > gridconst.D5camMinOffset) ? tempGoal.getOffset(): gridconst.D5camMinOffset);
            //if (XYZisfeasible(tempGoal))
            if(true)
            {
                double d5 = gridconst.D5cam + dOffset;
                double d6 = gridconst.D6cam ;
                double offsetXY = gridconst.L4 + (d6 * Math.Sin(theta5));
                Debug.WriteLine("L4 + cam offset: " + offsetXY);
                double EE = Math.Sqrt((X * X) + (Y * Y));
                if (EE > (gridconst.L0 + gridconst.L23max + d5))
                {
                    Debug.WriteLine("Cannot reach position " + X + "," + Y + "," + Z + " with view distance");
                    Debug.Write("View distance changed from " + dOffset);
                    dOffset += EE - (gridconst.L0 + gridconst.L23max + d5) + 50.0;
                    Debug.WriteLine(" to " + dOffset);
                    d5 = gridconst.D5point + dOffset;
                }
                double theta1 = 0.0;
                double theta2 = 0.0;
                double theta3 = 0.0;
                double theta4 = 0.0;
                double Cp = Math.Cos(thetap);
                double Sp = Math.Sin(thetap);
                double EEa = EE - (d5 * Cp) - (d6 * Sp) - gridconst.L0;
                double EEo = Z - (d5 * Sp) + (d6 * Cp) - gridconst.L1;
                double EEh2 = (EEa * EEa) + (EEo * EEo);
                double EEh = Math.Sqrt(EEh2);
                if (EEh > (gridconst.L23max))
                {
                    Debug.WriteLine("Cannot reach with current grip orientation");
                    double EEat = EE - gridconst.L0;
                    double EEot = Z - gridconst.L1;
                    double EEht = Math.Sqrt((EEat * EEat) + (EEot * EEot));
                    thetap = Math.Acos(((EEht * EEht) + (d5 * d5) - (gridconst.L23max * gridconst.L23max)) / (2.0 * EEht * d5)) - Math.Acos(EEat / EEht);
                    Debug.WriteLine("Setting gripper pitch to " + gridconst.toDegrees(thetap) + " degrees");
                    Cp = Math.Cos(thetap);
                    Sp = Math.Sin(thetap);
                    EEa = EE - (d5 * Cp) - (d6 * Sp) - gridconst.L0;
                    EEo = Z + (d5 * Sp) - (d6 * Cp) - gridconst.L1;
                    EEh = gridconst.L23max;
                    EEh2 = EEh * EEh;
                    theta3 = Math.PI;

                }
                else
                {
                    theta3 = Math.Acos(((gridconst.L2 * gridconst.L2) + (gridconst.L3 * gridconst.L3) - EEh2) / (2.0 * gridconst.L2 * gridconst.L3));
                }
                //if (theta3 > gridconst.toRadians(gridconst.theta3max))
                //{
                //    Debug.WriteLine("theta3 above max");
                //    theta3 = gridconst.toRadians(gridconst.theta3max);
                //}
                //else if (theta3 < gridconst.toRadians(gridconst.theta3min))
                //{
                //    Debug.WriteLine("theta3 below min");
                //    theta3 = gridconst.toRadians(gridconst.theta3min);
                //}

                theta1 = Math.Atan2(Y, X) + Math.Asin(offsetXY / EE);
                theta2 = Math.Acos(EEa / EEh);
                theta2 += Math.Acos(((gridconst.L2 * gridconst.L2) + (EEh * EEh) - (gridconst.L3 * gridconst.L3)) / (2.0 * gridconst.L2 * EEh));
                theta4 = (2.0 * Math.PI) - theta2 - theta3 + thetap;
                tempGoal.setJoint(0, (gridconst.toDegrees(theta1)));// + gridconst.theta1fudge));
                tempGoal.setJoint(1, -gridconst.toDegrees(theta2));
                tempGoal.setJoint(2, 180.0 - gridconst.toDegrees(theta3)); //180.0 + for elbow down config
                tempGoal.setJoint(3, 180.0 - gridconst.toDegrees(theta4));
                //tempGoal.joint[4] = gridconst.toDegrees(theta5);
                //Debug.Write("X:" + tempGoal.pos[0] + " Y:" + tempGoal.pos[1] + " Z:" + tempGoal.pos[1]);
                //Debug.Write(" J0:" + tempGoal.getJoint(0) + " J1:" + tempGoal.getJoint(1) + " J2:" + tempGoal.getJoint(2));
                //Debug.WriteLine(" J3:" + tempGoal.getJoint(3) + " J4:" + tempGoal.getJoint(4) + " W:" + tempGoal.getPitch());

                return true;
            }
            else
            {
                Debug.WriteLine(tempGoal.pos[0] + "," + tempGoal.pos[1] + "," + tempGoal.pos[2] + " outside workspace");
                Console.WriteLine(tempGoal.pos[0] + "," + tempGoal.pos[1] + "," + tempGoal.pos[2] + " outside workspace");
                return false;
            }

        }
        public static bool solvePointToJointAngles(PathPoint tempGoal)
        {
            double X = tempGoal.pos[0];
            double Y = tempGoal.pos[1];
            double Z = tempGoal.pos[2];
            double thetap = gridconst.toRadians(tempGoal.getPitch());
            double theta5 = gridconst.toRadians(tempGoal.getRoll());
            double dOffset = ((tempGoal.getOffset() > gridconst.D5pointMinOffset) ? tempGoal.getOffset() : gridconst.D5pointMinOffset);
            //if (XYZisfeasible(tempGoal))
            if(true)
            {
                double d5 = gridconst.D5point + dOffset;
                double d6 = gridconst.D6point;
                double offsetXY = gridconst.L4 - (d6 * Math.Sin(theta5));
                double EE = Math.Sqrt((X * X) + (Y * Y));
                if (EE > (gridconst.L0 + gridconst.L23max + d5))
                {
                    Debug.WriteLine("Cannot reach position " + X + "," + Y + "," + Z + " with current offset");
                    Debug.Write("Offset changed from " + dOffset);
                    dOffset += EE - (gridconst.L0 + gridconst.L23max + d5) + 50.0;
                    Debug.WriteLine(" to " + dOffset);
                    d5 = gridconst.D5point + dOffset;
                }
                double theta1 = 0.0;
                double theta2 = 0.0;
                double theta3 = 0.0;
                double theta4 = 0.0;
                double Cp = Math.Cos(thetap);
                double Sp = Math.Sin(thetap);
                double EEa = EE - (d5 * Cp) - (d6 * Sp) - gridconst.L0;
                double EEo = Z - (d5 * Sp) + (d6 * Cp) - gridconst.L1;
                double EEh2 = (EEa * EEa) + (EEo * EEo);
                double EEh = Math.Sqrt(EEh2);
                if (EEh > (gridconst.L23max))
                {
                    Debug.WriteLine("Cannot reach with current grip orientation");
                    double EEat = EE - gridconst.L0;
                    double EEot = Z - gridconst.L1;
                    double EEht = Math.Sqrt((EEat * EEat) + (EEot * EEot));
                    thetap = Math.Acos(((EEht * EEht) + (d5 * d5) - (gridconst.L23max * gridconst.L23max)) / (2.0 * EEht * d5)) - Math.Acos(EEat / EEht);
                    Debug.WriteLine("Setting gripper pitch to " + gridconst.toDegrees(thetap) + " degrees");
                    Cp = Math.Cos(thetap);
                    Sp = Math.Sin(thetap);
                    EEa = EE - (d5 * Cp) - (d6 * Sp) - gridconst.L0;
                    EEo = Z + (d5 * Sp) - (d6 * Cp) - gridconst.L1;
                    EEh = gridconst.L23max;
                    EEh2 = EEh * EEh;
                    theta3 = Math.PI;

                }
                else
                {
                    theta3 = Math.Acos(((gridconst.L2 * gridconst.L2) + (gridconst.L3 * gridconst.L3) - EEh2) / (2.0 * gridconst.L2 * gridconst.L3));
                }
                //if (theta3 > gridconst.toRadians(gridconst.theta3max))
                //{
                //    Debug.WriteLine("theta3 above max");
                //    theta3 = gridconst.toRadians(gridconst.theta3max);
                //}
                //else if (theta3 < gridconst.toRadians(gridconst.theta3min))
                //{
                //    Debug.WriteLine("theta3 below min");
                //    theta3 = gridconst.toRadians(gridconst.theta3min);
                //}

                theta1 = Math.Atan2(Y, X) + Math.Asin(offsetXY / EE);
                theta2 = Math.Acos(EEa / EEh);
                theta2 += Math.Acos(((gridconst.L2 * gridconst.L2) + (EEh * EEh) - (gridconst.L3 * gridconst.L3)) / (2.0 * gridconst.L2 * EEh));
                theta4 = (2.0 * Math.PI) - theta2 - theta3 + thetap;
                tempGoal.setJoint(0, (gridconst.toDegrees(theta1)));// + gridconst.theta1fudge));
                tempGoal.setJoint(1, -gridconst.toDegrees(theta2));
                tempGoal.setJoint(2, 180.0 - gridconst.toDegrees(theta3)); //180.0 + for elbow down config
                tempGoal.setJoint(3, 180.0 - gridconst.toDegrees(theta4));
                //tempGoal.joint[4] = gridconst.toDegrees(theta5);
                //Debug.Write("X:" + tempGoal.pos[0] + " Y:" + tempGoal.pos[1] + " Z:" + tempGoal.pos[1]);
                //Debug.Write(" J0:" + tempGoal.getJoint(0) + " J1:" + tempGoal.getJoint(1) + " J2:" + tempGoal.getJoint(2));
                //Debug.WriteLine(" J3:" + tempGoal.getJoint(3) + " J4:" + tempGoal.getJoint(4) + " W:" + tempGoal.getPitch());

                return true;
            }
            else
            {
                Debug.WriteLine(tempGoal.pos[0] + "," + tempGoal.pos[1] + "," + tempGoal.pos[2] + " outside workspace");
                Console.WriteLine(tempGoal.pos[0] + "," + tempGoal.pos[1] + "," + tempGoal.pos[2] + " outside workspace");
                return false;
            }

        }
        public static bool Cellisfeasible(int xcell, int ycell, int zcell)
        {
            double x, y, z;
            x = (double)(((xcell - gridconst.xcellsneg) * gridconst.GRIDRES) + (0.5 * gridconst.GRIDRES));
            y = (double)(((ycell - gridconst.ycellsneg) * gridconst.GRIDRES) + (0.5 * gridconst.GRIDRES));
            z = (double)(((zcell) * gridconst.GRIDRES) + (0.5*gridconst.GRIDRES));
            return XYZisfeasible(x, y, z);
        }
        public static bool XYZisfeasible(double x, double y, double z)
        {
//#if DEBUG
//            Debug.WriteLine("XYZisfeasible(double) [" + x + "," + y + "," + z + "]");
//#endif
            if (z < gridconst.minZcoord)
                return false;
            double XYradius = Math.Sqrt((x * x) + (y * y));
            if( (XYradius < gridconst.Baseradius) && ( z < gridconst.Baseheight ) )
                return false;
            //double theta1temp = gridconst.theta1fudge + Math.Atan2(y, x) + Math.Atan2(gridconst.L4, Math.Sqrt((XYradius * XYradius) - (gridconst.L4 * gridconst.L4)));
            double theta1temp = Math.Atan2(y, x) + Math.Asin(gridconst.L4 / XYradius) + gridconst.toRadians(gridconst.theta1fudge);
           
            if( (gridconst.toDegrees(theta1temp) < gridconst.theta1min) || (gridconst.toDegrees(theta1temp) > gridconst.theta1max) )
                return false;
//#if DEBUG
//            Debug.WriteLine("XYZ " + x + "," + y + "," + z + ", theta1 " + gridconst.toDegrees(theta1temp));
//#endif
            if( Math.Sqrt( ((XYradius - gridconst.L0) * (XYradius - gridconst.L0)) + ((z - gridconst.L1)*(z - gridconst.L1)) ) > (gridconst.L2 + gridconst.L3 + gridconst.D5grip))
                return false;
            return true;
        }
        public static bool XYZisfeasible(PathPoint pos)
        {
            //#if DEBUG
            //            Debug.WriteLine("XYZisfeasible(double) [" + x + "," + y + "," + z + "]");
            //#endif
            double x = pos.pos[0];
            double y = pos.pos[1];
            double z = pos.pos[2];

            double XYradius = Math.Sqrt((x * x) + (y * y));
            if (z < gridconst.minZcoord)
                return false;
            if ((XYradius < gridconst.Baseradius) && (z < gridconst.Baseheight))
                return false;
            double theta1temp = gridconst.toRadians(gridconst.theta1fudge) + Math.Atan2(y, x) + Math.Atan2(gridconst.L4, Math.Sqrt((XYradius * XYradius) - (gridconst.L4 * gridconst.L4)));
            if ((gridconst.toDegrees(theta1temp) < gridconst.theta1min) || (gridconst.toDegrees(theta1temp) > gridconst.theta1max))
                return false;
            //#if DEBUG
            //            Debug.WriteLine("XYZ " + x + "," + y + "," + z + ", theta1 " + gridconst.toDegrees(theta1temp));
            //#endif
            if (Math.Sqrt(((XYradius - gridconst.L0) * (XYradius - gridconst.L0)) + ((z - gridconst.L1) * (z - gridconst.L1))) > (gridconst.L2 + gridconst.L3 + gridconst.D5grip))
                return false;
            return true;
        }

        //#region old angle solvers
        //public static bool XYZsolveanglesGripper(double x, double y, double z, double griporientation) //MR need to pass in pointer to angles vector too
        //{
        //    double XYradius = Math.Sqrt((x * x) + (y * y));
        //    if( (XYradius < gridconst.Baseradius) && ( z < gridconst.Baseheight ) )
        //    {
        //        // x,y,z is inside base region
        //        return false;
        //    }
        //    double theta1 = gridconst.theta1fudge + Math.Atan2(y, x) + Math.Atan2(gridconst.L4, Math.Sqrt((XYradius * XYradius) - (gridconst.L4 * gridconst.L4)));
        //    if( (gridconst.toDegrees(theta1) < gridconst.theta1min) || (gridconst.toDegrees(theta1) > gridconst.theta1max) )
        //    {
        //        //theta1 too large or too small
        //        return false;
        //    }
        //    double XYZradius = Math.Sqrt( ((XYradius - gridconst.L0) * (XYradius - gridconst.L0)) + ((z - gridconst.L1)*(z - gridconst.L1)) );
        //    if (XYZradius > (gridconst.L2 + gridconst.L3 + gridconst.D5grip))
        //    {
        //        // x,y,z further then maximum length of arm
        //        return false;
        //    }
        //    if (XYZradius > (gridconst.L2 + gridconst.L3 + (gridconst.D5grip * Math.Cos(gridconst.toRadians(griporientation)))))
        //    {
        //        // x,y,z too far for requested grip orientation
        //        griporientation = Math.Acos((XYZradius - gridconst.L2 - gridconst.L3) / gridconst.D5grip);
        //    }
        //    //can be reached with requested orientation; begin solving angles
        //    double j13xy = (XYradius - gridconst.L0 - (gridconst.D5grip * Math.Cos(gridconst.toRadians(griporientation))));
        //    double j13z = (z - gridconst.L1 + (gridconst.D5grip * Math.Sin(gridconst.toRadians(griporientation))));
        //    double j13dist = Math.Sqrt( (j13xy * j13xy) + (j13z * j13z) );
        //    double theta3inner = (Math.Acos( (gridconst.L2 * gridconst.L2) + (gridconst.L3 * gridconst.L3) - (j13dist * j13dist) )/(2.0 * gridconst.L2* gridconst.L3));
        //    double theta213 = (Math.Acos( (gridconst.L2 * gridconst.L2) - (gridconst.L3 * gridconst.L3) + (j13dist * j13dist) )/(2.0 * gridconst.L2* gridconst.L3));
        //    double theta2 = (Math.Acos(j13xy/j13dist)) + theta213; //this is elbow up configuration
        //    double theta4inner = (2.0 * Math.PI) - theta2 - theta3inner - griporientation;
        //    double theta3 = theta3inner - Math.PI;
        //    double theta4 = theta4inner - Math.PI;
        //    // todo: commit theta1, theta2, theta3 and theta4 to angles vector
        //    return true;
        //}
        //public static bool XYZsolveanglesCamera(double x, double y, double z, double camorientation, double cameraroll) //MR need to pass in pointer to angles vector too
        //{
        //    // not implemented yet 30/6/11; currently returns same value as XYZsolveanglesGripper
        //    // need to compensate for D6 camera offset
        //    double theta5 = cameraroll;
        //    double XYradius = Math.Sqrt((x * x) + (y * y));
        //    if ((XYradius < gridconst.Baseradius) && (z < gridconst.Baseheight))
        //    {
        //        // x,y,z is inside base region
        //        return false;
        //    }
        //    double theta1 = gridconst.theta1fudge + Math.Atan2(y, x) + Math.Atan2(gridconst.L4+(gridconst.D6cam* Math.Sin(theta5)), Math.Sqrt((XYradius * XYradius) - (gridconst.L4 * gridconst.L4)));
        //    if ((gridconst.toDegrees(theta1) < gridconst.theta1min) || (gridconst.toDegrees(theta1) > gridconst.theta1max))
        //    {
        //        //theta1 too large or too small
        //        return false;
        //    }
        //    double XYZradius = Math.Sqrt(((XYradius - gridconst.L0) * (XYradius - gridconst.L0)) + ((z - gridconst.L1) * (z - gridconst.L1)));
        //    if (XYZradius > (gridconst.L2 + gridconst.L3 + gridconst.D5grip))
        //    {
        //        // x,y,z further then maximum length of arm
        //        return false;
        //    }
        //    if (XYZradius > (gridconst.L2 + gridconst.L3 + (gridconst.D5grip * Math.Cos(gridconst.toRadians(camorientation)))))
        //    {
        //        // x,y,z too far for requested grip orientation
        //        camorientation = Math.Acos((XYZradius - gridconst.L2 - gridconst.L3) / gridconst.D5grip);
        //    }
        //    //can be reached with requested orientation; begin solving angles
        //    double j13xy = (XYradius - gridconst.L0 - (gridconst.D5grip * Math.Cos(gridconst.toRadians(camorientation))));
        //    double j13z = (z - gridconst.L1 + (gridconst.D5grip * Math.Sin(gridconst.toRadians(camorientation))));
        //    double j13dist = Math.Sqrt((j13xy * j13xy) + (j13z * j13z));
        //    double theta3inner = (Math.Acos((gridconst.L2 * gridconst.L2) + (gridconst.L3 * gridconst.L3) - (j13dist * j13dist)) / (2.0 * gridconst.L2 * gridconst.L3));
        //    // negative theta3temp = elbow bent up
        //    // positive theta3temp = elbow bent down
        //    double theta213 = (Math.Acos((gridconst.L2 * gridconst.L2) - (gridconst.L3 * gridconst.L3) + (j13dist * j13dist)) / (2.0 * gridconst.L2 * gridconst.L3));
        //    double theta2 = (Math.Acos(j13xy / j13dist)) + theta213;
        //    double theta4inner = (2.0 * Math.PI) - theta2 - theta3inner - camorientation;
        //    double theta3 = theta3inner - Math.PI;
        //    double theta4 = theta4inner - Math.PI;
        //    // todo: commit theta1, theta2, theta3 and theta4 to angles vector
        //    return true;
        //}
        //#endregion

    }
}
